/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */



import net.sourceforge.jFuzzyLogic.FIS;

public class FuzzyController {
	/**
	 * @author Paweł Jankowski
	 */

    private FIS fis;
    private String fclProgram;

    public FuzzyController() {

        fclProgram = "FUNCTION_BLOCK InvertedPendulumNXT\n"+
                
		   "\n "+
    	           "VAR_INPUT\n "+
    	           "    gyro: REAL;\n "+
                   "    angle: REAL;\n "+
    	           "END_VAR\n "+
    	           "\n "+
    	           "VAR_OUTPUT\n "+
    	           "    motorspeed: REAL;\n "+
    	           "END_VAR\n "+

    	           "\n "+

    	           "FUZZIFY gyro\n "+
    	           "    TERM back_small :=  (-20, 0) (-10, 1) (-1, 0) ;\n "+
                   "    TERM zero :=  (-2, 0) (0, 1) (2, 0) ;\n "+
                   "    TERM forward_small :=  (1, 0) (10, 1) (20, 0) ;\n "+
    	           "END_FUZZIFY\n"+

                   "FUZZIFY angle\n "+
    	           "    TERM back_small :=  (-14, 0) (-7, 1) (0, 0) ;\n "+
                   "    TERM zero :=  (-1, 0) (0, 1) (1, 0) ;\n "+
                   "    TERM forward_small :=  (0, 0) (7, 1) (14, 0) ;\n "+
    	           "END_FUZZIFY\n"+

                   "\n" +

    	           "DEFUZZIFY motorspeed\n "+
                   "    TERM back_slow := (-100, 0) (-50, 1) (0, 0);\n "+
                   "    TERM back_veryslow := (-50, 0) (-25, 1) (0, 0);\n"+
                   "    TERM stop := (-1, 0) (0, 0) (1, 0);\n "+
                   "    TERM forward_veryslow := (0, 0) (25, 1) (50, 0);\n"+
                   "    TERM forward_slow := (0, 0) (50, 1) (100, 0);\n "+
    	           "    METHOD : COG;\n "+
    	           "    DEFAULT := 0;\n "+
                   "    RANGE := (-150 .. 150);\n"+
    	           "END_DEFUZZIFY\n "+

    	           "\n "+

    	           "RULEBLOCK first\n "+
    	           "    ACCU : MAX;\n "+
    	           "    AND : MIN;\n "+

                   "    RULE 1 : IF angle IS back_small AND gyro IS back_small THEN motorspeed IS back_slow;\n "+
                   "    RULE 2 : IF angle IS back_small AND gyro IS zero THEN motorspeed IS back_slow;\n "+
                   "    RULE 3 : IF angle IS back_small AND gyro IS forward_small THEN motorspeed IS back_veryslow;\n "+

                   "    RULE 4 : IF angle IS zero AND gyro IS back_small THEN motorspeed IS back_veryslow;\n "+
                   "    RULE 5 : IF angle IS zero AND gyro IS zero THEN motorspeed IS stop;\n "+
                   "    RULE 6 : IF angle IS zero AND gyro IS forward_small THEN motorspeed IS forward_veryslow;\n "+

                   "    RULE 7 : IF angle IS forward_small AND gyro IS back_small THEN motorspeed IS forward_veryslow;\n"+
                   "    RULE 8 : IF angle IS forward_small AND gyro IS zero THEN motorspeed IS forward_slow;\n"+
                   "    RULE 9 : IF angle IS forward_small AND gyro IS forward_small THEN motorspeed IS forward_slow;\n"+

//                   "    RULE 1 : IF gyro IS back_small THEN motorspeed IS back_veryslow;\n "+
//                   "    RULE 2 : IF gyro IS zero THEN motorspeed IS stop;\n "+
//                   "    RULE 3 : IF gyro IS forward_small THEN motorspeed IS forward_veryslow;\n "+
                   
                   "\n "+
    	           "END_RULEBLOCK\n "+
                   
    	           "\n "+
    	           "END_FUNCTION_BLOCK\n";
        try{
            this.fis = FIS.createFromString(fclProgram, true);
        }
        catch(Exception e){
            System.err.println(e.toString());
        }
    }

    public void setInput(int gyro, int angle){
        fis.setVariable("gyro", gyro);
        fis.setVariable("angle", angle);
    }

    public int getOutput(){
        fis.evaluate();
        //fis.chart();
        int output = (int)fis.getVariable("motorspeed").getLatestDefuzzifiedValue();
        return output;
    }
}
